package robocup.component.geometry;

import java.util.LinkedList;

import robocup.component.geometry.analysers.ReachableArea;

/**
 * @author Thomas Karbe
 * 
 */
public class LineSegment {

  private final Vektor        point1      = new Vektor();
  private final Vektor        point2      = new Vektor(Vektor.XY, 0, 1);

  /** ************************** DUMMIES ************************* */

  private final Vektor        dummyVektor = Vektor.getDummyInstance();

  /** ************************** STATICS ************************* */

  /**
   * "epsilon"
   */
  private final static double E           = 0.000001;

  /** ************************** CONSTRUCTORS ******************** */

  /**
   * Creates a dummy-instance of a LineSegment.
   * 
   * <br>
   * NEW OBJECTS: 3+this
   */
  public LineSegment() {

  }

  /**
   * Creates a lineSegment between point1 and point2. Vektors are copied.
   * 
   * <br>
   * NEW OBJECTS: 3+this
   * 
   * @param point1
   *          the start point
   * @param point2
   *          the end point
   * @throws IllegalArgumentException
   *           if distance between point1 and point2 is 0.
   */
  public LineSegment(
      final Vektor point1,
      final Vektor point2) {

    if (point1.getDistance(point2) > 0) {
      this.point1.copy(point1);
      this.point2.copy(point2);
    }
    else {
      throw new IllegalArgumentException(
          "Distance between point1 and point2 is 0");
    }
  }

  /** ************************** STATICS ************************* */

  /**
   * Creates a dummy-instance of a LineSegment
   * 
   * <br>
   * NEW OBJECTS: 4
   * 
   * @return a dummy instance
   */
  public static final LineSegment getDummyInstance() {

    return new LineSegment();
  }

  /** ************************** METHODS ************************* */

  /**
   * Copies the values from the given LineSegment.
   * 
   * @param ls
   *          the lineSegment to copy
   */
  public void copy(final LineSegment ls) {

    point1.copy(ls.point1);
    point2.copy(ls.point2);
  }

  /**
   * Returns a duplicate of this LineSegment
   * 
   * <br>
   * NEW OBJECTS: 4
   * 
   * @return a duplicate of this object
   */
  public final LineSegment cloned() {

    return new LineSegment(this.point1, this.point2);
  }

  /**
   * set the LineSegment to dummy values
   */
  public void reset() {

    point1.x = 0;
    point1.y = 0;
    point2.x = 0;
    point2.y = 1;
  }

  /**
   * sets the point1 to position
   * 
   * @param position
   *          the position to set
   */
  public void copyPoint1(final Vektor position) {

    point1.copy(position);
    if (point1.getDistance(point2) == 0) {
      point1.x += 0.000001;
    }
  }

  /**
   * sets the point2 to position
   * 
   * @param position
   *          the position to set
   */
  public void copyPoint2(final Vektor position) {

    point2.copy(position);
    if (point1.getDistance(point2) == 0) {
      point2.x += 0.000001;
    }
  }

  /**
   * @return point 1
   */
  public final Vektor getPoint1() {

    return point1;
  }

  /**
   * @return point 2
   */
  public final Vektor getPoint2() {

    return point2;
  }

  /**
   * @return the distance between point 1 and point 2
   */
  public final double getLength() {

    return point1.getDistance(point2);
  }

  /**
   * @return the angle from point 1 to point 2
   */
  public final double getAngle() {

    return point1.getAngleTo(point2);
  }

  /**
   * Moves point 2 to a position with the given angle relative to point 1 and
   * without changing the distance.
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param angle
   *          the needed angle of the line
   */
  public void setAngle(final double angle) {

    dummyVektor.setVektor(getLength(), angle);
    point2.copy(point1);
    point2.addToThis(dummyVektor);
  }

  /**
   * Moves point 2 to a position with the given distance to point 1 and without
   * changing the angle.
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param dist
   *          the needed distance
   */
  public void setDistance(final double dist) {

    if (dist <= 0) {
      throw new IllegalArgumentException("dist must be bigger than 0");
    }
    dummyVektor.setVektor(dist, getAngle());
    point2.copy(point1);
    point2.addToThis(dummyVektor);
  }

  /**
   * Moves point 2 to a position with the given distance and angle relative to
   * point 1.
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param dist
   *          the needed distance
   * @param angle
   *          the needed angle
   */
  public void setPolar(final double dist,
      final double angle) {

    if (dist <= 0) {
      throw new IllegalArgumentException("dist must be bigger than 0");
    }
    dummyVektor.setVektor(dist, angle);
    point2.copy(point1);
    point2.addToThis(dummyVektor);
  }

  /**
   * Calculates the intersection point between the two LineSegments and adds it
   * to the list.
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param line
   *          the LineSegment to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointToList(final LineSegment line,
      final LinkedList<Vektor> list) {

    Vektor v = getIntersectionPoint(line);
    if (v != null) {
      list.add(v);
    }
  }

  /**
   * Calculates the intersection point between this LineSegment and a
   * StraightLine and adds it to the list
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param line
   *          the StraightLine to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointToList(final StraightLine line,
      final LinkedList<Vektor> list) {

    Vektor v = getIntersectionPoint(line);
    if (v != null) {
      list.add(v);
    }
  }

  /**
   * Calculates the intersection points between this LineSegment and a Circle
   * and adds them to the list.
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param circ
   *          the Circle to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointsToList(final Circle circ,
      final LinkedList<Vektor> list) {

    LinkedList<Vektor> list2 = getIntersectionPoints(circ);
    for (Vektor v : list2) {
      if (v != null) {
        list.add(v);
      }
    }
  }

  /**
   * Calculates the intersection points between this LineSegment and a
   * CircleSegment and adds them to the list.
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param circ
   *          the Circle to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointsToList(final CircleSegment circ,
      final LinkedList<Vektor> list) {

    LinkedList<Vektor> list2 = getIntersectionPoints(circ);
    for (Vektor v : list2) {
      if (v != null) {
        list.add(v);
      }
    }
  }

  /**
   * Calculates the intersection point between the two LineSegments. Sets the
   * result to empty if there is no intersection point.
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param line
   *          the LineSegment to intersect with
   * @param result
   *          here the result is saved (overwrites old contents)
   */
  public void getIntersectionPoint(final LineSegment line,
      final Vektor result) {

    double a1 = point2.x - point1.x;
    double b1 = point2.y - point1.y;
    double a2 = line.point2.x - line.point1.x;
    double b2 = line.point2.y - line.point1.y;
    if (b2 * a1 - b1 * a2 == 0) {
      result.setEmpty();
      return;
    }
    double s = (line.point1.x * b1 - point1.x * b1 - line.point1.y * a1 + point1.y
        * a1)
        / (b2 * a1 - b1 * a2);
    dummyVektor.x = line.point1.x + s * a2;
    dummyVektor.y = line.point1.y + s * b2;
    if (point1.getDistance(dummyVektor) > getLength()) {
      result.setEmpty();
      return;
    }
    if (point2.getDistance(dummyVektor) > getLength()) {
      result.setEmpty();
      return;
    }
    if (line.point1.getDistance(dummyVektor) > line.getLength()) {
      result.setEmpty();
      return;
    }
    if (line.point2.getDistance(dummyVektor) > line.getLength()) {
      result.setEmpty();
      return;
    }
    result.copy(dummyVektor);
  }

  /**
   * Calculates the intersection point between the two LineSegments. The result
   * is null if there is no intersection point.
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param line
   *          the LineSegment to intersect with
   * @return the intersection point or null
   */
  public final Vektor getIntersectionPoint(final LineSegment line) {

    Vektor res = new Vektor();
    getIntersectionPoint(line, res);
    return res.isEmpty() ? null : res;
  }

  /**
   * Calculates the intersection point between this LineSegment and a
   * StraightLine. Sets the result to empty if there is no intersection point.
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param line
   *          the StraightLine to intersect with
   * @param result
   *          here the result is saved (overwrites old contents)
   */
  public void getIntersectionPoint(final StraightLine line,
      final Vektor result) {

    // System.out.println("\n\ncalculate intersection points between:");
    // System.out.println("LINESEGMENT "+this);
    // System.out.println("STRAIGHTLINE "+line+"\n");

    if (point1.x == point2.x) {
      // System.out.println("LS is parallel to Y");
      if (line.getM() == Double.POSITIVE_INFINITY) {
        // System.out.println("SL is parallel to Y");
        // System.out.println("NO INTERSECTION");
        result.setEmpty();
        return;
      }
    }
    else {
      // System.out.println("LS is not parallel to Y");
      final double thisM = (point2.y - point1.y) / (point2.x - point1.x);
      if (thisM + LineSegment.E >= line.getM()
          && thisM - LineSegment.E <= line.getM()) {
        // System.out.println("LS is parallel to SL");
        // System.out.println("NO INTERSECTION");
        result.setEmpty();
        return;
      }
    }
    if (line.getM() == Double.POSITIVE_INFINITY) {
      // System.out.println("SL is parallel to Y");
      result.x = line.getN();
      result.y = point1.y
          + ((line.getN() - point1.x) / (point2.x - point1.x))
          * (point2.y - point1.y);
      if (!isOnLineSegment(result)) {
        // System.out.println("result: "+result+" is NOT on LS");
        result.setEmpty();
      }
      // System.out.println("result: "+result);
      return;
    }
    final double lambda = (point1.y - line.getM() * point1.x - line.getN())
        / (line.getM() * point2.x - line.getM() * point1.x - point2.y + point1.y);
    // System.out.println("lambda= "+lambda);
    if (lambda > 1 || lambda < 0) {
      // System.out.println("lambda is not between 0 and 1 --> NO RESULT");
      result.setEmpty();
      return;
    }
    result.x = point1.x + lambda * (point2.x - point1.x);
    result.y = point1.y + lambda * (point2.y - point1.y);
    // System.out.println("result: "+result);
  }

  /**
   * Calculates the intersection point between this LineSegment and a
   * StraightLine. The result is null if there is no intersection point.
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param line
   *          the StraightLine to intersect with
   * @return the intersection point or null
   */
  public final Vektor getIntersectionPoint(final StraightLine line) {

    Vektor res = new Vektor();
    getIntersectionPoint(line, res);
    return res.isEmpty() ? null : res;
  }

  /**
   * Calculates the intersection points of the LineSegment and the given Circle.
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param c
   *          the circle to intersect with
   * @return a LinkedList containing all intersection points
   */
  public final LinkedList<Vektor> getIntersectionPoints(final Circle c) {

    LinkedList<Vektor> list = new LinkedList<Vektor>();
    getIntersectionPoints(c, list);
    return list;
  }

  /**
   * Calculates the intersection points of the LineSegment and the given Circle.
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * <br>
   * NEW OBJECTS <= 2
   * 
   * @param c
   *          the circle to intersect with
   * @param result
   *          contents is overwritten. includes 0-2 Vektors
   */
  public final void getIntersectionPoints(final Circle c,
      final LinkedList<Vektor> result) {

    if (!result.isEmpty()) {
      result.clear();
    }

    // System.out.println("\n\ncalculate intersection points between:");
    // System.out.println("LINESEGMENT "+this);
    // System.out.println("CIRCLE "+c+"\n");

    getClosestPointOnStraightLine(c.getCenter(), dummyVektor);
    // System.out.println("closest point on StraightLine: "+dummyVektor);
    final double closeDist = dummyVektor.getDistance(c.getCenter());
    // System.out.println("dist: "+closeDist);
    if (closeDist > c.getRadius()) {
      // System.out.println("no intersection");
      return;
    }
    if (closeDist == c.getRadius()) {
      if (isOnLineSegment(dummyVektor)) {
        result.add(dummyVektor.cloned());
        // System.out.println("close point is IP");
        return;
      }
      // System.out.println("no intersection");
    }
    final double kath = Math.sqrt(c.getRadius()
        * c.getRadius()
        - closeDist
        * closeDist);
    // System.out.println("kath: "+kath);
    Vektor p1 = point2.sub(point1);
    // System.out.println("p1: "+p1);
    Vektor p2 = p1.cloned();
    p1.divThis(p1.getLength());
    // System.out.println("p1 normalized: "+p1);
    p1.multThis(kath);
    // System.out.println("p1 set length kath: "+p1);
    p1.addToThis(dummyVektor);
    // System.out.println("p1 added close point: "+p1);
    if (isOnLineSegment(p1)) {
      // DUMMYVEKTOR is overwritten in isOnLineSegment!!!
      result.add(p1);
      // System.out.println("p1 is IP");
    }
    else {
      // System.out.println("p1 is not IP");
    }
    p2.divThis(p2.getLength());
    // System.out.println("p2 normalized: "+p2);
    p2.multThis(-kath * 2);
    // System.out.println("p2 set length kath: "+p2);
    p2.addToThis(p1);
    // System.out.println("p2 added close point: "+p2);
    if (isOnLineSegment(p2)) {
      result.add(p2);
      // System.out.println("p2 is IP");
    }
    else {
      // System.out.println("p2 is not IP");
    }
  }

  /**
   * Calculates the intersection points of the LineSegment and the given
   * CircleSegment.
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param c
   *          the CircleSegment to intersect with
   * @return a LinkedList containing all intersection points
   */
  public final LinkedList<Vektor> getIntersectionPoints(final CircleSegment c) {

    return c.getIntersectionPoints(this);
  }

  /**
   * Calculates the intersection points of the LineSegment and the given
   * CircleSegment.
   * 
   * <br>
   * NEW OBJECTS <= 2
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param c
   *          the CircleSegment to intersect with
   * @param result
   *          contents is overwritten. includes 0-2 Vektors
   */
  public final void getIntersectionPoints(final CircleSegment c,
      final LinkedList<Vektor> result) {

    c.getIntersectionPoints(this, result);
  }
  
  /**
   * Calculates the intersection points of this LineSegment and a RA and add the results to the given list.
   * 
   * @param ra the RA to intersect with
   * @param back if true, backwards movement is allowed
   * @return a list that contains the intersection points
   */
  public final LinkedList<Vektor> getIntersectionPoints(final ReachableArea ra, final boolean back) {
    return ra.getIntersectionPoints(this, back);
  }
  
  /**
   * Calculates the intersection points of this LineSegment and a RA and add the results to the given list.
   * 
   * @param ra the RA to intersect with
   * @param back if true, backwards movement is allowed
   * @param list here the results are added
   */
  public final void addIntersectionPointsToList(final ReachableArea ra, final boolean back, final LinkedList<Vektor> list) {
    ra.addIntersectionPointsToList(this, back, list);
  }

  /**
   * Calculates the closest point to pos that is on the infinite extension of
   * this LineSegment
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param pos
   *          the close position
   * @return the closest point
   */
  public final Vektor getClosestPointOnStraighLine(final Vektor pos) {

    Vektor res = new Vektor();
    getClosestPointOnStraightLine(pos, res);
    return res;
  }

  /**
   * Calculates the closest point to pos that is on the infinite extension of
   * this LineSegment
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param pos
   *          the close position
   * @param result
   *          here the resulting position is saved, old values are overwritten
   */
  public final void getClosestPointOnStraightLine(final Vektor pos,
      final Vektor result) {

    if (point1.y == point2.y) {
      result.y = point1.y;
      result.x = pos.x;
      return;
    }
    if (point1.x == point2.x) {
      result.x = point1.x;
      result.y = pos.y;
      return;
    }
    final double xp = point1.x;
    final double yp = point1.y;
    final double xq = point2.x;
    final double yq = point2.y;
    final double xs = pos.x;
    final double ys = pos.y;
    final double lambda = (xp
        * xp
        - xp
        * xq
        + ys
        * yq
        - ys
        * yp
        - xp
        * xs
        + xq
        * xs
        - yp
        * yq + yp * yp)
        / (yq * yq - 2 * yp * yq + yp * yp - 2 * xp * xq + xp * xp + xq * xq);
    result.x = point1.x + lambda * (point2.x - point1.x);
    result.y = point1.y + lambda * (point2.y - point1.y);
  }

  /**
   * Calculates the closest point to pos on the LineSegment
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param pos
   *          the close position
   * @return the closest point
   */
  public final Vektor getClosestPointOnLineSegment(final Vektor pos) {

    Vektor res = new Vektor();
    getClosestPointOnLineSegment(pos, res);
    return res;
  }

  /**
   * Calculates the closest point to pos on the LineSegment
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param pos
   *          the close point
   * @param result
   *          here the resulting position is saved, old values are overwritten
   */
  public final void getClosestPointOnLineSegment(final Vektor pos,
      final Vektor result) {

    getClosestPointOnStraightLine(pos, dummyVektor);
    final double length = this.getLength();
    final double dist1 = dummyVektor.getDistance(point1);
    final double dist2 = dummyVektor.getDistance(point2);
    if (length >= dist1 && length >= dist2) {
      result.copy(dummyVektor);
      return;
    }
    if (dist1 >= dist2) {
      result.copy(point2);
      return;
    }
    result.copy(point1);
  }

  /**
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param pos
   *          the position to check
   * @return true if pos is on the infinite extension of this line
   */
  public final boolean isOnStraightLine(final Vektor pos) {

    getClosestPointOnStraightLine(pos, dummyVektor);
    if (dummyVektor.getDistance(pos) < LineSegment.E) {
      return true;
    }
    return false;
  }

  /**
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param pos
   *          the position to check
   * @return true if pos is on this LineSegment
   */
  public final boolean isOnLineSegment(final Vektor pos) {

    getClosestPointOnStraightLine(pos, dummyVektor);
    if (dummyVektor.getDistance(pos) < LineSegment.E) {
      final double length = getLength();
      if (length >= pos.getDistance(point1)
          && length >= pos.getDistance(point2)) {
        return true;
      }
    }
    return false;
  }

  /**
   * 
   * @see java.lang.Object#toString()
   */
  public final String toString() {

    StringBuffer sb = new StringBuffer();
    sb.append("p1: ").append(point1).append(" p2: ").append(point2).append(
        " len: ").append(getLength()).append(" ang: ").append(getAngle());
    return sb.toString();
  }

  /**
   * <br>
   * NEW OBJECTS: 2
   * 
   * @return the StraightLine that covers this LineSegment
   */
  public final StraightLine getStraightLine() {

    return new StraightLine(point1, point2);
  }

  /**
   * @param result
   *          old contents will be deleted. The new contents will be the
   *          StraightLine that covers this LineSegment.
   */
  public void getStraightLine(StraightLine result) {

    result.setStraightLine(point1, point2);
  }
}
